#ifndef DIRECT_METHOD_H
#define DIRECT_METHOD_H

// #define FMT_HEADER_ONLY
// #include "fmt/format.h"

#include <opencv2/opencv.hpp>
// #include <sophus/se3.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <mutex>
#include <vector>
#include <boost/format.hpp>

namespace MultiColSLAM
{

    using VecVector2d = std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>>;

    // typedefs
    typedef Eigen::Matrix<double, 6, 6> Matrix6d;
    typedef Eigen::Matrix<double, 2, 6> Matrix26d;
    typedef Eigen::Matrix<double, 6, 1> Vector6d;

    // Camera intrinsics
    extern double fx, fy, cx, cy;
    // baseline
    extern double baseline;
    // paths
    extern std::string left_file;
    extern std::string disparity_file;
    extern boost::format fmt_others;

    /// Jacobian Accumulator for parallel computation of Jacobians
    class JacobianAccumulator
    {
    public:
        JacobianAccumulator(const cv::Mat &img1_, const cv::Mat &img2_, const VecVector2d &px_ref_,
                            const std::vector<double> depth_ref_, Eigen::Isometry3d &T21_);

        void accumulate_jacobian(const cv::Range &range);

        Matrix6d hessian() const;
        Vector6d bias() const;
        double cost_func() const;
        VecVector2d projected_points() const;
        void reset();

    private:
        const cv::Mat &img1, &img2;
        const VecVector2d &px_ref;
        const std::vector<double> depth_ref;
        Eigen::Isometry3d &T21;
        VecVector2d projection;
        std::mutex hessian_mutex;
        Matrix6d H = Matrix6d::Zero();
        Vector6d b = Vector6d::Zero();
        double cost = 0;
    };

    class DirectMethod
    {
    public:
        DirectMethod();
        double DirectPoseEstimationMultiLayer(const cv::Mat &img1, const cv::Mat &img2,
                                            const VecVector2d &px_ref, const std::vector<double> depth_ref,
                                            Eigen::Isometry3d &T21);

        double DirectPoseEstimationSingleLayer(const cv::Mat &img1, const cv::Mat &img2,
                                             const VecVector2d &px_ref, const std::vector<double> depth_ref,
                                             Eigen::Isometry3d &T21);
    };

    float GetPixelValue(const cv::Mat &img, float x, float y);

    class JacobianAccumulatorLoopBody : public cv::ParallelLoopBody
    {
    public:
        JacobianAccumulatorLoopBody(JacobianAccumulator* jaco_accu)
            : jaco_accu_(jaco_accu) {}

        virtual void operator()(const cv::Range& range) const override {
            jaco_accu_->accumulate_jacobian(range);
        }

    private:
        JacobianAccumulator* jaco_accu_;
    };
}
#endif // DIRECT_METHOD_H
